//
// Created by PulsarV on 19-2-26.
//

#ifdef USE_DSP_DEVICE

#include <rc_move/rcmove.h>
#include <cstring>

void RC::RobotCarMove::wheel_1_backward(int speed) {
//    this->serial_device.ansy_send(RC_WHEEL_1_BACKWARD);
}

void RC::RobotCarMove::wheel_1_forward(int speed) {
//    this->serial_device->send(RC_WHEEL_1_FORWARD);

}

void RC::RobotCarMove::wheel_2_backward(int speed) {
//    this->serial_device->send(RC_WHEEL_2_BACKWARD);

}

void RC::RobotCarMove::wheel_2_forward(int speed) {
//    this->serial_device->send(RC_WHEEL_2_FORWARD);
}

void RC::RobotCarMove::wheel_3_backward(int speed) {
//    this->serial_device->send(RC_WHEEL_3_BACKWARD);

}

void RC::RobotCarMove::wheel_3_forward(int speed) {
//    this->serial_device->send(RC_WHEEL_3_FORWARD);
}

void RC::RobotCarMove::wheel_AC(int speed_1, int speed_2, int speed_3) {
    WHEEL_DATA wheelData = {speed_1, speed_2, speed_3, 1, 1, 1};
    char *data=(char *)malloc(sizeof(WHEEL_DATA));
    memset(data,0,sizeof(WHEEL_DATA));
    memcpy(data,&wheelData, sizeof(WHEEL_DATA));

    this->serial_device.ansy_send(data, sizeof(WHEEL_DATA));
}

void RC::RobotCarMove::wheel_CW(int speed_1, int speed_2, int speed_3) {
    WHEEL_DATA wheelData = {speed_1, speed_2, speed_3, 0, 0, 0};
    char *data=(char *)malloc(sizeof(WHEEL_DATA));
    memset(data,0,sizeof(WHEEL_DATA));
    memcpy(data,&wheelData, sizeof(WHEEL_DATA));

    this->serial_device.ansy_send(data, sizeof(WHEEL_DATA));
}

void RC::RobotCarMove::wheel_go_forward(int speed_1, int speed_2) {
    WHEEL_DATA wheelData = {3413333, 3413333, 0, 0, 1, 1};
    char *data=(char *)malloc(sizeof(WHEEL_DATA));
    memset(data,0,sizeof(WHEEL_DATA));
    memcpy(data,&wheelData, sizeof(WHEEL_DATA));
    this->serial_device.ansy_send(data, sizeof(WHEEL_DATA));

}

void RC::RobotCarMove::wheel_go_backward(int speed_1, int speed_2) {
    WHEEL_DATA wheelData = {3413333, 3413333, 0, 1, 0, 1};
    char *data=(char *)malloc(sizeof(WHEEL_DATA));
    memset(data,0,sizeof(WHEEL_DATA));
    memcpy(data,&wheelData, sizeof(WHEEL_DATA));

    this->serial_device.ansy_send(data, sizeof(WHEEL_DATA));
}

void RC::RobotCarMove::wheel_stop() {
    WHEEL_DATA wheelData = {0, 0, 0, 1, 1, 1};
    char *data=(char *)malloc(sizeof(WHEEL_DATA));
    memset(data,0,sizeof(WHEEL_DATA));
    memcpy(data,&wheelData, sizeof(WHEEL_DATA));
    this->serial_device.ansy_send(data, sizeof(WHEEL_DATA));
}

void RC::RobotCarMove::excute(WHEEL_DATA wheelData) {
    wheel_stop();

    if (wheelData.wheel_1_speed != 0) {
        if (wheelData.weel_1_direction == RC_MOVE_FORWARD) {
            wheel_1_forward(wheelData.wheel_1_speed);
        } else
            wheel_1_backward(wheelData.wheel_1_speed);
    }
    if (wheelData.wheel_2_speed != 0) {
        if (wheelData.weel_2_direction == RC_MOVE_FORWARD) {
            wheel_2_forward(wheelData.wheel_2_speed);
        } else
            wheel_2_backward(wheelData.wheel_2_speed);
    }

    if (wheelData.wheel_3_speed != 0) {
        if (wheelData.weel_3_direction == RC_MOVE_FORWARD) {
            wheel_3_forward(wheelData.wheel_3_speed);
        } else
            wheel_3_backward(wheelData.wheel_3_speed);
    }
}
#endif
